﻿using System.Buffers;
using UnityEngine;


namespace Panda.Utils.Physics
{

    public class CollisionDetection
    {
        // 检测两个OBB是否碰撞
        public static bool CheckCollision(OBB box1, OBB box2)
        {
            const int count = 15;
            Vector3[] axes = ArrayPool<Vector3>.Shared.Rent(count);
            {
                GetAxes(box1, box2, axes);

                for (var i = 0; i < count; i++)
                {
                    var axis = axes[i];
                    if (!OverlapOnAxis(box1, box2, axis))
                    {
                        ArrayPool<Vector3>.Shared.Return(axes);
                        return false; // 找到一个分离轴，盒子不碰撞
                    }
                }
            }
            ArrayPool<Vector3>.Shared.Return(axes);

            return true; // 所有轴重叠，盒子碰撞
        }

        // 获取所有可能的分离轴
        private static void GetAxes(OBB box1, OBB box2, Vector3[] axesResult15Element)
        {
            int index = 0;

            // 添加每个盒子的本地轴
            axesResult15Element[index++] = box1.Right;
            axesResult15Element[index++] = box1.Up;
            axesResult15Element[index++] = box1.Forward;
            axesResult15Element[index++] = box2.Right;
            axesResult15Element[index++] = box2.Up;
            axesResult15Element[index++] = box2.Forward;

            const int count = 3;
            var a1 = ArrayPool<Vector3>.Shared.Rent(count);
            var a2 = ArrayPool<Vector3>.Shared.Rent(count);
            {
                box1.GetAxes(a1);
                box2.GetAxes(a2);
            
                // 添加两盒子边的叉积
                for (var i = 0; i <count; i++)
                {
                    var axis1 = a1[i];
                    for (var i1 = 0; i1 < count; i1++)
                    {
                        var axis2 = a2[i1];
                        axesResult15Element[index++] = Vector3.Cross(axis1, axis2);
                    }
                }
            }
            ArrayPool<Vector3>.Shared.Return(a1);
            ArrayPool<Vector3>.Shared.Return(a2);
        }

        // 检查在给定轴上的投影是否重叠
        private static bool OverlapOnAxis(OBB box1, OBB box2, Vector3 axis)
        {
            (float min1, float max1) = Project(box1, axis);
            (float min2, float max2) = Project(box2, axis);

            return !(min1 > max2 || min2 > max1);
        }

        // 计算盒子在给定轴上的投影
        private static (float, float) Project(OBB box, Vector3 axis)
        {
            float min = float.MaxValue;
            float max = float.MinValue;

            const int count = 8;
            var vertices = ArrayPool<Vector3>.Shared.Rent(count);
            {
                box.GetVertices(vertices);
                for (var i = 0; i < count; i++)
                {
                    var vertex = vertices[i];
                    float projection = Vector3.Dot(vertex, axis);
                    min = Mathf.Min(min, projection);
                    max = Mathf.Max(max, projection);
                }
            }
            ArrayPool<Vector3>.Shared.Return(vertices);

            return (min, max);
        }

        public static bool CheckCollision(Sphere sphere, OBB obb)
        {
            return CheckCollision(obb, sphere);
        }

        public static bool CheckCollision(OBB obb, Sphere sphere)
        {
            // 找到球体中心在OBB上的最近点
            Vector3 closestPoint = FindClosestPointOnOBB(sphere.Center, obb);

            // 计算球体中心到最近点的距离
            float distanceSquared = (closestPoint - sphere.Center).sqrMagnitude;

            // 检查距离是否小于或等于球体的半径的平方
            return distanceSquared <= sphere.Radius * sphere.Radius;
        }

        private static Vector3 FindClosestPointOnOBB(Vector3 point, OBB obb)
        {
            Vector3 closestPoint = obb.Center;

            const int count = 3;
            // 将点投影到OBB的每个轴上并限制到盒子的范围内
            Vector3[] axes = ArrayPool<Vector3>.Shared.Rent(count);
            Vector3[] halfSizes = ArrayPool<Vector3>.Shared.Rent(count);
            {
                obb.GetAxes(axes);
                
                halfSizes[0] = obb.Right * obb.HalfSize.x;
                halfSizes[1] = obb.Up * obb.HalfSize.y;
                halfSizes[2] = obb.Forward * obb.HalfSize.z;

                for (int i = 0; i < count; i++)
                {
                    Vector3 axis = axes[i];
                    float projection = Vector3.Dot(point - obb.Center, axis);
                    float clampedProjection = Mathf.Clamp(projection, -halfSizes[i].magnitude, halfSizes[i].magnitude);
                    closestPoint += clampedProjection * axis;
                }
            }
            ArrayPool<Vector3>.Shared.Return(axes);
            ArrayPool<Vector3>.Shared.Return(halfSizes);

            return closestPoint;
        }
        
        public static bool CheckCollision(Sphere sphere1, Sphere sphere2)
        {
            // 计算两个球体中心之间的距离的平方
            float distanceSquared = (sphere1.Center - sphere2.Center).sqrMagnitude;

            // 计算两个球体半径之和的平方
            float radiusSum = sphere1.Radius + sphere2.Radius;
            float radiusSumSquared = radiusSum * radiusSum;

            // 检查距离平方是否小于或等于半径之和的平方
            return distanceSquared <= radiusSumSquared;
        }
        
        public static bool CheckCollision(AABB a, AABB b)
        {
            // Check for overlap on the x-axis
            if (Mathf.Abs(a.Center.x - b.Center.x) > (a.HalfSize.x + b.HalfSize.x))
                return false;

            // Check for overlap on the y-axis
            if (Mathf.Abs(a.Center.y - b.Center.y) > (a.HalfSize.y + b.HalfSize.y))
                return false;

            // Check for overlap on the z-axis
            if (Mathf.Abs(a.Center.z - b.Center.z) > (a.HalfSize.z + b.HalfSize.z))
                return false;

            // If all axes overlap, the AABBs intersect
            return true;
        }

        public static bool CheckCollision(Sphere sphere, AABB aabb)
        {
            return CheckCollision(aabb, sphere);
        }

        public static bool CheckCollision(AABB aabb, Sphere sphere)
        {
            // Find the closest point on the AABB to the sphere center
            float closestX = Mathf.Max(aabb.Center.x - aabb.HalfSize.x, Mathf.Min(sphere.Center.x, aabb.Center.x + aabb.HalfSize.x));
            float closestY = Mathf.Max(aabb.Center.y - aabb.HalfSize.y, Mathf.Min(sphere.Center.y, aabb.Center.y + aabb.HalfSize.y));
            float closestZ = Mathf.Max(aabb.Center.z - aabb.HalfSize.z, Mathf.Min(sphere.Center.z, aabb.Center.z + aabb.HalfSize.z));

            // Calculate the distance between the sphere's center and this closest point
            float distanceX = sphere.Center.x - closestX;
            float distanceY = sphere.Center.y - closestY;
            float distanceZ = sphere.Center.z - closestZ;

            // Calculate the squared distance and compare it to the squared radius
            float distanceSquared = distanceX * distanceX + distanceY * distanceY + distanceZ * distanceZ;
            return distanceSquared <= (sphere.Radius * sphere.Radius);
        }
    }

    // 定义OBB类
    public struct OBB
    {
        public Vector3 Center;
        public Vector3 Right;
        public Vector3 Up;
        public Vector3 Forward;
        public Vector3 HalfSize;

        public void GetAxes(Vector3[] axesResult3Element)
        {
            axesResult3Element[0] = Right;
            axesResult3Element[1] = Up;
            axesResult3Element[2] = Forward;
        }

        public void GetVertices(Vector3[] verticesResult8Element)
        {
            // 计算方向向量的缩放
            Vector3 scaledRight = Right * HalfSize.x;
            Vector3 scaledUp = Up * HalfSize.y;
            Vector3 scaledForward = Forward * HalfSize.z;

            // 计算八个顶点
            verticesResult8Element[0] = Center + scaledRight + scaledUp + scaledForward;
            verticesResult8Element[1] = Center + scaledRight + scaledUp - scaledForward;
            verticesResult8Element[2] = Center + scaledRight - scaledUp + scaledForward;
            verticesResult8Element[3] = Center + scaledRight - scaledUp - scaledForward;
            verticesResult8Element[4] = Center - scaledRight + scaledUp + scaledForward;
            verticesResult8Element[5] = Center - scaledRight + scaledUp - scaledForward;
            verticesResult8Element[6] = Center - scaledRight - scaledUp + scaledForward;
            verticesResult8Element[7] = Center - scaledRight - scaledUp - scaledForward;
        }
        
        public OBB(Vector3 position, Quaternion rotation, Vector3 sizeValue) {
            Center = position;
            
            Vector3 right1 = rotation * Vector3.right;
            Vector3 up1 = rotation * Vector3.up;
            Vector3 forward1 = rotation * Vector3.forward;
            
            Forward = forward1; Right = right1; Up = up1;
            HalfSize = sizeValue * 0.5f;
        }
        
    }
    
    public struct AABB
    {
        public Vector3 Center;
        public Vector3 HalfSize;

        public AABB(Vector3 center, Vector3 halfSize)
        {
            Center = center;
            HalfSize = halfSize;
        }
    }
    
    public struct Sphere
    {
        public Vector3 Center;
        public float Radius;

        public Sphere(Vector3 center, float radius)
        {
            Center = center;
            Radius = radius;
        }
    }


}